
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       esc_calibration.c
  * @author     baiyang
  * @date       2022-1-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <rtthread.h>

#include "fms.h"

#include <parameter/param.h>
#include <board_config/board_config.h>
#include <srv_channel/srv_channel.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <notify/notify.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/
#define ESC_CALIBRATION_HIGH_THROTTLE   950
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void fms_esc_calibration_passthrough();
static void fms_esc_calibration_auto();
static void fms_esc_calibration_notify();
static void fms_esc_calibration_setup();
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// check if we should enter esc calibration mode
void fms_esc_calibration_startup_check()
{
    param_link_variable(PARAM_ID(VEHICLE, ESC_CALIBRATION), &fms.g.esc_calibrate);

    if (mb_motors_is_brushed_pwm_type(fms.motors)) {
        // ESC cal not valid for brushed motors
        return;
    }

    // delay up to 2 second for first radio input
    uint8_t i = 0;
    while ((i++ < 100) && (fms.last_radio_update_ms == 0)) {
        rt_thread_mdelay(20);
        fms_read_radio();
    }

    // exit immediately if pre-arm rc checks fail
    if (!mb_arming_rc_calibration_checks(true)) {
        // clear esc flag for next time
        if ((fms.g.esc_calibrate != ESCCAL_NONE) && (fms.g.esc_calibrate != ESCCAL_DISABLED)) {
            param_set_and_save(PARAM_ID(VEHICLE, ESC_CALIBRATION), ESCCAL_NONE);
        }
        return;
    }

    // check ESC parameter
    switch (fms.g.esc_calibrate) {
        case ESCCAL_NONE:
            // check if throttle is high
            if (fms.channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {

                // we will enter esc_calibrate mode on next reboot
                param_set_and_save(PARAM_ID(VEHICLE, ESC_CALIBRATION), ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH);
                // send message to gcs
                gcs_send_text(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board");

                //
                console_printf("[ESC calibration]: Restart board\n");

                // turn on esc calibration notification
                notify_flags.esc_calibration = true;
                // block until we restart
                while(1) {
                    rt_thread_mdelay(100);
                }
            }
            break;
        case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH:
            // check if throttle is high
            if (fms.channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) {
                // pass through pilot throttle to escs
                fms_esc_calibration_passthrough();
            }
            break;
        case ESCCAL_PASSTHROUGH_ALWAYS:
            // pass through pilot throttle to escs
            fms_esc_calibration_passthrough();
            break;
        case ESCCAL_AUTO:
            // perform automatic ESC calibration
            fms_esc_calibration_auto();
            break;
        case ESCCAL_DISABLED:
        default:
            // do nothing
            break;
    }

    // clear esc flag for next time
    if (fms.g.esc_calibrate != ESCCAL_DISABLED) {
        param_set_and_save(PARAM_ID(VEHICLE, ESC_CALIBRATION), ESCCAL_NONE);
    }
}

// esc_calibration_passthrough - pass through pilot throttle to escs
static void fms_esc_calibration_passthrough()
{
    // send message to GCS
    gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs");

    fms_esc_calibration_setup();

    // 
    console_printf("[ESC calibration]: Passing pilot throttle to ESCs\n");

    while(1) {
        // flash LEDs
        fms_esc_calibration_notify();

        // read pilot input
        fms_read_radio();

        // we run at high rate to make oneshot ESCs happy. Normal ESCs
        // will only see pulses at the RC_SPEED
        rt_thread_mdelay(3);

        // pass through to motors
        srv_channels_cork();
        mb_motors_set_throttle_passthrough_for_esc_calibration(fms.motors, fms.channel_throttle->control_in/1000.0f);
        srv_channels_push();
    }
}

// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
static void fms_esc_calibration_auto()
{
    // send message to GCS
    gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: esc_calibration_auto");

    fms_esc_calibration_setup();

    // 
    console_printf("[ESC calibration]: esc_calibration_auto\n");

    // raise throttle to maximum
    srv_channels_cork();
    mb_motors_set_throttle_passthrough_for_esc_calibration(fms.motors, 1.0f);
    srv_channels_push();

    // delay for 5 seconds while outputting pulses
    uint32_t tstart = time_millis();
    while (time_millis() - tstart < 5000) {
        srv_channels_cork();
        mb_motors_set_throttle_passthrough_for_esc_calibration(fms.motors, 1.0f);
        srv_channels_push();
        fms_esc_calibration_notify();
        rt_thread_mdelay(3);
    }

    // block until we restart
    while(1) {
        srv_channels_cork();
        mb_motors_set_throttle_passthrough_for_esc_calibration(fms.motors, 0.0f);
        srv_channels_push();
        fms_esc_calibration_notify();
        rt_thread_mdelay(3);
    }
}

// flash LEDs to notify the user that ESC calibration is happening
static void fms_esc_calibration_notify()
{
    notify_flags.esc_calibration = true;
    uint32_t now = time_millis();
    if (now - fms.esc_calibration_notify_update_ms > 20) {
        fms.esc_calibration_notify_update_ms = now;
        notify_update();
    }
}

static void fms_esc_calibration_setup()
{
    // clear esc flag for next time
    param_set_and_save(PARAM_ID(VEHICLE, ESC_CALIBRATION), ESCCAL_NONE);

    if (mb_motors_is_normal_pwm_type(fms.motors)) {
        // run at full speed for oneshot ESCs (actually done on push)
        mb_motors_set_update_rate(fms.motors, fms.g.rc_speed);
    } else {
        // reduce update rate to motors to 50Hz
        mb_motors_set_update_rate(fms.motors, 50);
    }

    // disable safety if requested
    //BoardConfig.init_safety();

    // wait for safety switch to be pressed
    uint32_t tstart = 0;
    while (srv_hal_safety_switch_state() == SAFETY_DISARMED) {
        const uint32_t tnow = time_millis();
        if (tnow - tstart >= 5000) {
            gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch");
            console_printf("[ESC calibration]: Push safety switch\n");
            tstart = tnow;
        }
        fms_esc_calibration_notify();
        rt_thread_mdelay(3);
    }

    // arm and enable motors
    fms.motors->_armed = true;
    srv_channels_enable_by_mask(mb_motors_get_motor_mask(fms.motors));
    brd_set_soft_armed(true);
}
/*------------------------------------test------------------------------------*/


